/* user.c
 *
 * VERSION: 1.0  (18 Mar 2010)
 *
 * DESCRIPTION:
 * Edit this file as you like. Please do not change the provided
 * function names, but you are free to add as many new functions
 * as needed.
 *
 * Feel free to include additional source files, too. 
 * Don't forget to add *.c files to the Makefile!
*/

#include "user.h"
#include "devices.h"
#include "servo.h"
#include "analog.h"
#include "SPI.h"
#include "LCD.h"	// for debugging

void pid(void);

/* This is called ONCE before entering the main loop.
 */ 
void userInit() {
	// Initialize motor values to NEUTRAL = 127
	trex[0].motor[0] = 127;
	trex[0].motor[1] = 127;
	trex[0].motor[2] = 127;
	trex[1].motor[0] = 127;
	trex[1].motor[1] = 127;
	trex[1].motor[2] = 127;
	x2.motor[0] = 127;
	x2.motor[1] = 127;

	// [1 of 2] Uncomment to enable X2 servos
	// Servo 0 is pin D4 and servo 1 is pin D5
	//servo_init();
	//servo_enable(0);
	//servo_enable(1);
	
	// Encoder check
	int i, j, sensorValue;
	char *sensorString;
	sensorString = malloc(sizeof(int) * 8);
	
	
	sc.servo[1] = 155;
	
	/*
	for(i = 0; i < 255; i++) {
		sc.servo[1] = i;
		sensorValue = analog8(0);
		sensorString = itoa(sensorValue, sensorString);
		
		LCDString("Output:");
		LCDAddString(sensorString);
		
		for (j = 0 ; j < 250000; j++) {
		}
	}
	*/
	
}


/* This is called every iteration of the main loop while the
 * robot is in Autonomous mode. The frequency at which it's called
 * is similar to the userLoopFast() function (i.e. as fast as possible).
 */
void userAutoLoop(void) {
	
}


/* This is called every time data is received from the Rabbit
 * via WiFi.
 */
void userLoop() {
	// Example using the LCD to print debug values
	LCDClear();
	for (unsigned char i = 0; i < 4; i += 1) {
		LCDHex(controls.analog[i]);
		LCDSendData(' ');
	}

	/////////////////////////
	// Drive code example  //
	/////////////////////////

	// Simple "tank drive" example:
	// Use the L joystick's y-axis to control the L motor
	// and the R joystick's y-axis to control the R motor

	// Tank drive using the TReX
	//trex[0].motor[0] = controls.analog[1];
	//trex[1].motor[1] = controls.analog[3];

	// Tank drive using the X2
	//  (see below on how to control the X2 motor directly)
	//x2.motor[0] = controls.analog[1];
	//x2.motor[1] = controls.analog[3];


	///////////////////////////
	// Servo control example //
	///////////////////////////

	// FIRST: Uncomment the TWO sections of code marked 
	//  "Uncomment to enable X2 servos"
	// This maps servos 0 and 1 to the X and Y axes (respectively) of the
	//  second controller
	// (Servo 0 is pin D4 and servo 1 is pin D5: refer to the diagram in
	//  the Electronics Assembly Guide, "Connecting the X2" section)
	//x2.servo[0] = controls.analog[4];
	//x2.servo[1] = controls.analog[5];



	//////////////////////////////////////////////////////////////////
    // Code below this point actually sets motor and servo values.  //
    // Make sure you know what you're doing before modifying!       //
	//////////////////////////////////////////////////////////////////

	// [2 of 2] Uncomment to enable X2 servos
	//servo_setPos(0, x2.servo[0]);
	//servo_setPos(1, x2.servo[1]);

	// This scales the values in x2.motor[] to the range [-255,255]
	//  needed by setMotorN()
	int motorVal;

	motorVal = ((int)x2.motor[0] - 127) * 2;
	setMotor1(motorVal ? motorVal-1 : 0);
	motorVal = ((int)x2.motor[1] - 127) * 2;
	setMotor2(motorVal ? motorVal-1 : 0);

	// For advanced users: The setMotor1() and setMotor2() functions
	//  are defined in SPI.c. Look in that file for functions
	//  that do more advanced motor control (acceleration, braking).
	/*
	trex[0].motor[0] = controls.analog[6];
	trex[0].motor[1] = controls.analog[0];
	trex[1].motor[0] = controls.analog[2];
	trex[1].motor[1] = controls.analog[4];
	
	pid();
	*/
}

void pid() {
	static uint8_t last10inputs[4][10];

	static uint8_t error[4];
	
	static uint8_t prevError[4];
	static uint8_t allError[4];
	int i;
	for(int i = 0; i < 4; i++) {
		error[i] = 0;
	}
	for(int i = 0; i < 9; i++) {
		last10inputs[0][i+1] = last10inputs[0][i];
		last10inputs[1][i+1] = last10inputs[1][i];
		last10inputs[2][i+1] = last10inputs[2][i];
		last10inputs[3][i+1] = last10inputs[3][i];
	}
	last10inputs[0][0] = controls.analog[1];
	last10inputs[1][0] = controls.analog[3];
	last10inputs[2][0] = controls.analog[5];
	last10inputs[3][0] = controls.analog[7];
	
	int Kp = 1;
	int Kd = 1;
	int Ki = 1;
	int Ko = 1;
	
	int avg0, avg1, avg2, avg3;
	for(int i = 0; i < 10; i++) {
		avg0 += last10inputs[0][i];
		avg1 += last10inputs[1][i];
		avg2 += last10inputs[2][i];
		avg3 += last10inputs[3][i];
	}
	
	error[0] = avg0 - analog10(0);
	error[1] = avg1 - analog10(1);
	error[2] = avg2 - analog10(2);
	error[3] = avg3 - analog10(3);
	
	sc.servo[0] = Kp * error[0] + Kd * (error[0] - prevError[0]) + Ki * allError[0] / Ko;
	sc.servo[1] = Kp * error[1] + Kd * (error[1] - prevError[1]) + Ki * allError[1] / Ko;
	sc.servo[2] = Kp * error[2] + Kd * (error[2] - prevError[2]) + Ki * allError[2] / Ko;
	sc.servo[3] = Kp * error[3] + Kd * (error[3] - prevError[3]) + Ki * allError[3] / Ko;
	
	prevError[0] = error[0];
	allError[0] += error[0];
	
	prevError[1] = error[1];
	allError[1] += error[1];
	
	prevError[2] = error[2];
	allError[2] += error[2];
	
	prevError[3] = error[3];
	allError[3] += error[3];
	
	/*
	sc.servo[0] = controls.analog[1];
	sc.servo[1] = controls.analog[3];
	sc.servo[2] = controls.analog[5];
	sc.servo[3] = controls.analog[7];
	*/
}

/*
int DoPid(MotorInfo *p)
{
    int Perror, output;
 
    // Set point is an 24.8 number (+/- 1 mile@ 130/inch)
 
    Perror = (p->Setpoint>>8) - p->Encoder;
 
    // Derivative error is the delta Perror over Td
 
    output = (p->Kp*Perror + p->Kd*(Perror - p->PrevErr) + p->Ki*p->Ierror)/p->Ko;
    p->PrevErr = Perror;
 
    // Accumulate Integral error *or* Limit output.
    // Stop accumulating when output saturates
    
    if (output >= MAXOUTPUT)
        output = MAXOUTPUT;
    else if (output <= -MAXOUTPUT)
        output = -MAXOUTPUT;
    else
        p->Ierror += Perror;
    
    // Convert output to 0xFF-0x7F-0x00 (Fwd, neutral, Rev)
    // Locked anti-phase drive.
    
    return (output - (MAXOUTPUT+1));
}
*/



/* This is called every iteration of the main loop while the
 * robot is in Enabled mode. As the name suggests, this function
 * is called much more frequently than userLoop() !
 */
void userLoopFast() {
	
}
